/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "dma.h"
#include "spi.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "headfiles.h"
#include "pid.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

// 定义目标角度宏
#define TARGET_ANGLE 3100

// 声明 PID 句柄
PID_HandleTypeDef pid_position_motor1;
PID_HandleTypeDef pid_speed_motor1;

// 声明 PID 参数结构体
extern Motor_PID_Params motor1_pid_params;

int16_t MotorCurrent[4] = {0};
uint8_t ETYETX;
uint8_t BCT;
uint8_t FW;

float factor[20];
struct fix_point_s fix_points_flash[130] = {0};
int enc_zero_tmp = 0;
float speed_target = 200;
int check_flag = 0;
int get_fix_point = 0;
int count1 = -2, count2 = 0;


/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */



/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_CAN1_Init();
  MX_CAN2_Init();
  MX_SPI1_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */

    CAN_FilterConfig();

    MA600_HandleInit(&TestMA600, &hspi1, SPI1_CS_GPIO_Port, SPI1_CS_Pin);
    
    // ETYETX = MA600_Read_ETYETX(&TestMA600);
    
    // BCT = MA600_Read_BCT(&TestMA600);
    
    // FW = MA600_Read_FW(&TestMA600);
    
    // ETYETX = MA600_Set_ETYETX(&TestMA600, 0, 1);
    
    // BCT = MA600_Set_BCT(&TestMA600, 94);
    
    // FW = MA600_Set_FW(&TestMA600, 8);
    
    // MotorCurrent[0] = 140;
    
    // fix_handle_init(&fix_handle_test, 64, fix_type_null);
    
    // PID_InitTypeDef PID_ENC_INIT = {0};
    
    // PID_ENC_INIT.mode = PID_MODE_POSITION;
    // PID_ENC_INIT.outMax = 2000;
    // PID_ENC_INIT.iOutMax = 1000;
    // PID_ENC_INIT.kp = 2.5;
    // PID_ENC_INIT.ki = 0.1;
    // PID_ENC_INIT.kd = 1;
    
    // PID_init(&motor_pid.Rudder_ENC_speed, &PID_ENC_INIT);

    // 初始化位置环 PID
    PID_Init(&pid_position_motor1, PID_MODE_POSITION);
    PID_SetParam(&pid_position_motor1, motor1_pid_params.position);
    PID_SetTarget(&pid_position_motor1, TARGET_ANGLE);

    // 初始化速度环 PID
    PID_Init(&pid_speed_motor1, PID_MODE_POSITION);
    PID_SetParam(&pid_speed_motor1, motor1_pid_params.speed);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
      HAL_Delay(1);
      
      PID_calc(&motor_pid.Rudder_ENC_speed, speed_target, motor_data.Rudder.speed);
      MotorCurrent[0] = motor_pid.Rudder_ENC_speed.out;
      
      CAN_TX_CAN2_0x200(MotorCurrent[0], MotorCurrent[1], MotorCurrent[2], MotorCurrent[3]);
      
//      MA600_Get_Speed_rpm(&TestMA600);
      
//      factor[0] = motor_data.Rudder.position / 8192.0f;
//      factor[1] = TestMA600.Angle / 65536.0f;
//      
//      factor[3] = factor[2];
//      
//      factor[2] = factor[0] - factor[1];
//      
//      factor[4] = (ABS(factor[3] - factor[2]) < 0.5) ? (factor[3] - factor[2]) : 0;
//      factor[4] = (factor[4] * 360);
      
      // if((TestMA600.Angle < 10 || TestMA600.Angle > 65536 - 10) && check_flag)
      // {
      //     enc_zero_tmp = motor_data.Rudder.position;
      //     check_flag = 0;
          
      //     get_fix_point = 2;
      // }
          
      // if(count1 == -1)
      // {
      //     fix_load(&fix_handle_test, fix_points_flash, fix_check);
      //     fix_handle_test.fix_type = fix_type_interpolation;
      //     count1 = -2;
      // }
      
      // 获取传感器反馈值，例如 current_angle
      /* 从 MA600 获取当前角度和速度 */
      MA600_Get_Speed_rpm(&TestMA600);
      float current_angle = TestMA600.Angle;
      float motor_speed = TestMA600.Speed;

      // 位置环计算
      PID_SetTarget(&pid_position_motor1, TARGET_ANGLE);
      float speed_setpoint = PID_Calc(&pid_position_motor1, current_angle);

      // 速度环计算，使用 PID_calc 函数
      float motor_output = PID_calc(&pid_speed_motor1, speed_setpoint, motor_speed);

      // 控制电机输出
      MotorCurrent[0] = (int16_t)motor_output;
      CAN_TX_CAN1_0x200(MotorCurrent[0], MotorCurrent[1], MotorCurrent[2], MotorCurrent[3]);
      
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.Prediv1Source = RCC_PREDIV1_SOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  RCC_OscInitStruct.PLL2.PLL2State = RCC_PLL_NONE;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure the Systick interrupt time
  */
  __HAL_RCC_PLLI2S_ENABLE();
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
